// ros2 topic pub /point_topic geometry_msgs/msg/Point "{x: 819.7, y: 3550.7, z: 2030.5}" --rate 1
// ros2 topic pub /point_topic geometry_msgs/msg/Point "{x: 778.7, y: 3372.7, z: 2730.5}" --rate 1
// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float64.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include <iostream>
#include <unistd.h>
#include "cansend.h"

using namespace std::chrono_literals;

int can0_socket = -1; // CAN0 socket描述符
uint32_t ext_id = 0x0c0ac2c3;

// 运行模式（正常运行 or 急停）
int run_model = 0;
int send_state = 0;

int joint1gp;
int joint2gp;
int joint3gp;
int joint4gp;

int flag_joint1 = 0;
int flag_joint2 = 0;
int flag_joint3 = 0;
int flag_joint4 = 0;
// 扫刷标志
int endeffector = 0;

// 台号1
BYTE nazzle_valves1[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};
BYTE OPEN_1_1[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X01};
BYTE OPEN_1_2[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X02, 0X01};
BYTE OPEN_1_3[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X03, 0X01};
BYTE OPEN_1_4[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X04, 0X01};
BYTE CLOSE_1[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};
// 台号2
BYTE nazzle_valves2[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};
BYTE OPEN_2_1[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X01};
BYTE OPEN_2_2[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X02, 0X01};
BYTE OPEN_2_3[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X03, 0X01};
BYTE OPEN_2_4[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X04, 0X01};
BYTE CLOSE_2[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};

// float joint2p;
// PID控制参数
float Kp1_1; // 上升 // 比例系数
float Kp1_2; // 下降
float Kp2_1; // 上升 // 比例系数
float Kp2_2; // 下降
float Kp3_1; // 上升 // 比例系数
float Kp3_2; // 下降
float Kp4_1; // 上升 // 比例系数
float Kp4_2; // 下降

// 3.自定义节点类；
class Arm_Control : public rclcpp::Node
{
public:
  Arm_Control() : Node("arm_control_joint_node_cpp")
  {
    // 实例化回调组
    callback_group_0 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_1 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_2 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_3 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_4 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_5 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_6 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_7 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_8 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_9 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_10 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

    // 创建订阅选项，并设置回调组
    rclcpp::SubscriptionOptions options_0;
    options_0.callback_group = callback_group_0;
    rclcpp::SubscriptionOptions options_1;
    options_1.callback_group = callback_group_1;
    rclcpp::SubscriptionOptions options_2;
    options_2.callback_group = callback_group_2;
    rclcpp::SubscriptionOptions options_3;
    options_3.callback_group = callback_group_3;
    rclcpp::SubscriptionOptions options_4;
    options_4.callback_group = callback_group_4;
    rclcpp::SubscriptionOptions options_5;
    options_5.callback_group = callback_group_5;
    rclcpp::SubscriptionOptions options_6;
    options_6.callback_group = callback_group_6;
    rclcpp::SubscriptionOptions options_7;
    options_7.callback_group = callback_group_7;
    rclcpp::SubscriptionOptions options_8;
    options_8.callback_group = callback_group_8;
    rclcpp::SubscriptionOptions options_9;
    options_9.callback_group = callback_group_9;
    rclcpp::SubscriptionOptions options_10;
    options_10.callback_group = callback_group_10;

    updata_group = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
    rclcpp::SubscriptionOptions valves_;
    valves_.callback_group = updata_group;

    // 扫刷控制
    this->enaction_sub_ = this->create_subscription<std_msgs::msg::Int32>("end_action", 10, std::bind(&Arm_Control::enaction_cb, this, std::placeholders::_1), options_10);
    this->run_model_sub_ = this->create_subscription<std_msgs::msg::Int32>("run_model_sub", 10, std::bind(&Arm_Control::run_model_cb, this, std::placeholders::_1), options_10);
    // 订阅来自编码器/ADC的消息
    this->joint1p_sub_ = this->create_subscription<std_msgs::msg::Int32>("joint1_p", 10, std::bind(&Arm_Control::control_joint1cb, this, std::placeholders::_1), options_4);
    this->joint2p_sub_ = this->create_subscription<std_msgs::msg::Float64>("joint2_p", 10, std::bind(&Arm_Control::control_joint2cb, this, std::placeholders::_1), options_2);
    this->joint3p_sub_ = this->create_subscription<std_msgs::msg::Float64>("joint3_p", 10, std::bind(&Arm_Control::control_joint3cb, this, std::placeholders::_1), options_6);
    this->joint4p_sub_ = this->create_subscription<std_msgs::msg::Int32>("joint4_p", 10, std::bind(&Arm_Control::control_joint4cb, this, std::placeholders::_1), options_8);

    // 订阅来自qt的消息 或 arm_constyle
    this->joint1gp_sub_ = this->create_subscription<std_msgs::msg::Float64>("joint1_gp", 10, std::bind(&Arm_Control::joint1gb_cb, this, std::placeholders::_1), options_5);
    this->joint2gp_sub_ = this->create_subscription<std_msgs::msg::Float64>("joint2_gp", 10, std::bind(&Arm_Control::joint2gb_cb, this, std::placeholders::_1), options_3);
    this->joint3gp_sub_ = this->create_subscription<std_msgs::msg::Float64>("joint3_gp", 10, std::bind(&Arm_Control::joint3gb_cb, this, std::placeholders::_1), options_7);
    this->joint4gp_sub_ = this->create_subscription<std_msgs::msg::Int32>("joint4_gp", 10, std::bind(&Arm_Control::joint4gb_cb, this, std::placeholders::_1), options_9);
    sendtimer_ = this->create_wall_timer(0.02s, std::bind(&Arm_Control::sendresult, this), callback_group_0);
  }

private:
  int state_first = 0;
  int state_second = 0;

  rclcpp::CallbackGroup::SharedPtr updata_group;

  // 动作类别
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr action_sub_;

  // 扫刷控制
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr enaction_sub_;

  // 发送模式控制
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr run_model_sub_;

  // 发送结果
  rclcpp::TimerBase::SharedPtr sendtimer_;

  // 来自编码器/ADC的消息
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint1p_sub_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint2p_sub_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint3p_sub_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint4p_sub_;

  // 来自qt的消息
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint1gp_sub_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint2gp_sub_;
  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint3gp_sub_;
  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr joint4gp_sub_;

  // 声明回调组
  rclcpp::CallbackGroup::SharedPtr callback_group_0;
  rclcpp::CallbackGroup::SharedPtr callback_group_1;
  rclcpp::CallbackGroup::SharedPtr callback_group_2;
  rclcpp::CallbackGroup::SharedPtr callback_group_3;
  rclcpp::CallbackGroup::SharedPtr callback_group_4;
  rclcpp::CallbackGroup::SharedPtr callback_group_5;
  rclcpp::CallbackGroup::SharedPtr callback_group_6;
  rclcpp::CallbackGroup::SharedPtr callback_group_7;
  rclcpp::CallbackGroup::SharedPtr callback_group_8;
  rclcpp::CallbackGroup::SharedPtr callback_group_9;
  rclcpp::CallbackGroup::SharedPtr callback_group_10;

  // ---------回调函数---------
  void control_joint1cb(const std_msgs::msg::Int32::SharedPtr joint1p_msg)
  {
    // RCLCPP_INFO(this->get_logger(), "JOINT1 CINTROL");
    int joint1p = joint1p_msg->data;
    // RCLCPP_INFO(this->get_logger(), "编码器1中获取的JOINT1位置消息joint1p_msg:%d", joint1p);
    // usleep(100000);
    if (flag_joint1 == 1)
    {
      // 向编码器数值增大的方向
      if (joint1gp - joint1p > 1)
      {
        if (endeffector == 0)
        {
          joint_move1[0] = joint1_r[0];
        }
        else if (endeffector == 1)
        {
          joint_move1[0] = joint1_ren[0];
        }
      }
      // 向编码器数值增减少的方向
      else if (joint1gp - joint1p < -1)
      {
        if (endeffector == 0)
        {
          joint_move1[0] = joint1_f[0];
        }
        else if (endeffector == 1)
        {
          joint_move1[0] = joint1_fen[0];
        }
      }
      else
      {
        // 停止
        if (endeffector == 0)
        {
          joint_move1[0] = joint_stop[0];
        }
        else if (endeffector == 1)
        {
          joint_move1[0] = 0x0f;
        }
        // usleep(100000);
        flag_joint1 = 0;
      }
    }
  }
  // ADC中获取的JOINT2位置消息 由于随着ADC信号更新，所以本身就是循环，不用添加其他循环
  void control_joint2cb(const std_msgs::msg::Float64::SharedPtr joint2p_msg)
  {
    // RCLCPP_INFO(this->get_logger(), "JOINT2 CINTROL");
    Kp2_1 = 1.8;
    Kp2_2 = 1.8;
    float joint2p = joint2p_msg->data;
    // RCLCPP_INFO(this->get_logger(), "ADC中获取的JOINT2位置消息joint2p_msg:%f", joint2p);
    // usleep(100000);
    if (flag_joint2 == 1)
    {
      float error = joint2gp - joint2p; // 计算位置误差
      float output_1;                   // 控制上升输出
      float output_2;                   // 控制下降输出

      output_1 = Kp2_1 * error; // 根据误差计算控制输出
      output_2 = Kp2_2 * error;
      // 根据实际位置控制机械臂动作
      // 允许的位移误差
      if (joint2gp - joint2p > 10)
      {
        // 上升
        // joint2_f[1] = static_cast<BYTE>(0x80 + std::min(127.0f, output_1)); // 这一句是为了单独调试时使用的，与 SendData(send_joint_v, joint_id, joint2_f, 4); 搭配
        joint_move2[1] = static_cast<BYTE>(0x80 + std::min(127.0f, output_1));
        // SendData(can0_socket, ext_id, true, joint2_f, 4);
      }
      else if (joint2gp - joint2p < -10)
      {
        // 下降
        // RCLCPP_INFO(this->get_logger(), "下降");
        joint_move2[1] = static_cast<BYTE>(0x80 - std::min(127.0f, -output_2));
      }
      else
      {
        // 停止
        joint_move2[1] = joint_stop[1];
        // usleep(100000);
        flag_joint2 = 0;
      }
    }
  }

  void control_joint3cb(const std_msgs::msg::Float64::SharedPtr joint3p_msg)
  {
    // RCLCPP_INFO(this->get_logger(), "JOINT3 CINTROL");
    Kp3_1 = 9.0;
    Kp3_2 = 1.0;
    float joint3p = joint3p_msg->data;
    // RCLCPP_INFO(this->get_logger(), "ADC中获取的JOINT2位置消息joint2p_msg:%f", joint3p);
    // usleep(100000);
    if (flag_joint3 == 1)
    {
      float error3 = joint3gp - joint3p; // 计算位置误差
      float output3_1;                   // 控制上升输出
      float output3_2;                   // 控制下降输出

      output3_1 = Kp3_1 * error3; // 根据误差计算控制输出
      output3_2 = Kp3_2 * error3;

      // 根据实际位置控制机械臂动作
      // 允许的位移误差
      if (joint3gp - joint3p > 10)
      {
        // 上升
        joint_move3[2] = static_cast<BYTE>(0x80 + std::min(127.0f, output3_1));
      }
      else if (joint3gp - joint3p < -10)
      {
        // 下降
        joint_move3[2] = static_cast<BYTE>(0x80 - std::min(127.0f, -output3_2));
      }
      else
      {
        // 停止
        joint_move3[2] = joint_stop[2];
        // usleep(100000);
        flag_joint3 = 0;
      }
    }
  }

  void control_joint4cb(const std_msgs::msg::Int32::SharedPtr joint4p_msg)
  {
    Kp4_1 = 1;
    Kp4_2 = 1;
    // RCLCPP_INFO(this->get_logger(), "JOINT1 CINTROL");
    int joint4p = joint4p_msg->data;
    // RCLCPP_INFO(this->get_logger(), "编码器2中获取的JOINT4位置消息joint1p_msg:%d", joint4p);
    // usleep(100000);
    if (flag_joint4 == 1)
    {
      float error = joint4gp - joint4p; // 计算位置误差
      // std::cout << "error = " << error << std::endl;
      float output4_1; // 控制上升输出
      float output4_2; // 控制下降输出
      // 根据误差计算控制输出
      output4_1 = Kp4_1 * error;
      output4_2 = Kp4_2 * error;

      if (joint4gp - joint4p > 1)
      {
        // 向编码器数值增大的方向
        joint_move4[3] = static_cast<BYTE>(0x80 + std::min(127.0f, output4_1));
      }
      else if (joint4gp - joint4p < -1)
      {
        // 向编码器数值增减少的方向
        joint_move4[3] = static_cast<BYTE>(0x80 - std::min(127.0f, -output4_2));
      }
      else
      {
        // 停止
        joint_move4[3] = joint_stop[3];
        // usleep(100000);
        flag_joint4 = 0;
      }
    }
  }

  // QT中下发的JOINT位置消息
  void joint1gb_cb(const std_msgs::msg::Float64::SharedPtr joint1gp_msg)
  {
    joint1gp = joint1gp_msg->data;
    flag_joint1 = 1;
    // RCLCPP_INFO(this->get_logger(), "joint1gp:%d", joint1gp);
  }
  void joint2gb_cb(const std_msgs::msg::Float64::SharedPtr joint2gp_msg)
  {
    joint2gp = joint2gp_msg->data;
    flag_joint2 = 1;
    // RCLCPP_INFO(this->get_logger(), "joint2gp:%d",joint2gp);
  }
  void joint3gb_cb(const std_msgs::msg::Float64::SharedPtr joint3gp_msg)
  {
    joint3gp = joint3gp_msg->data;
    flag_joint3 = 1;
    // RCLCPP_INFO(this->get_logger(), "joint2gp:%d",joint2gp);
  }
  void joint4gb_cb(const std_msgs::msg::Int32::SharedPtr joint4gp_msg)
  {
    joint4gp = joint4gp_msg->data;
    flag_joint4 = 1;
    // std::cout << "joint4gp" << joint4gp << std::endl;
    // RCLCPP_INFO(this->get_logger(), "joint4gp:%d",joint4gp);
  }
  void enaction_cb(const std_msgs::msg::Int32::SharedPtr action_msg)
  {
    endeffector = action_msg->data;
  }
  void run_model_cb(const std_msgs::msg::Int32::SharedPtr action_msg)
  {
    run_model = action_msg->data;
  }
  // 统一发送最终结果
  void sendresult()
  {
    int temp_runmode = 0;
    switch (run_model)
    // switch (temp_runmode)
    {
    // 泵关闭
    case 0:
    {
      // 一号关节
      joint_move[0] = joint_move1[0];
      // 二号关节
      joint_move[1] = joint_move2[1];
      // 三号关节
      joint_move[2] = joint_move3[2];
      // 四号关节
      joint_move[3] = joint_move4[3];
      // 泵
      joint_move[4] = close_pump[0];
      break;
    }
    // 低压泵开启
    case 1:
    {
      // 一号关节
      joint_move[0] = joint_move1[0];
      // 二号关节
      joint_move[1] = joint_move2[1];
      // 三号关节
      joint_move[2] = joint_move3[2];
      // 四号关节
      joint_move[3] = joint_move4[3];
      // 泵
      joint_move[4] = low_pump[0];
      break;
    }
    // 高压泵开启
    case 2:
    {
      // 一号关节
      joint_move[0] = joint_move1[0];
      // 二号关节
      joint_move[1] = joint_move2[1];
      // 三号关节
      joint_move[2] = joint_move3[2];
      // 四号关节
      joint_move[3] = joint_move4[3];
      // 泵
      joint_move[4] = high_pump[0];
      break;
    }
    // 全部开启
    case 3:
    {
      // 一号关节
      joint_move[0] = joint_move1[0];
      // 二号关节
      joint_move[1] = joint_move2[1];
      // 三号关节
      joint_move[2] = joint_move3[2];
      // 四号关节
      joint_move[3] = joint_move4[3];
      // 泵
      joint_move[4] = double_pump[0];
      break;
    }
    case 4:
    {
      // 急停
      joint_move[0] = joint_allstop[0];
      joint_move[1] = joint_allstop[1];
      joint_move[2] = joint_allstop[2];
      joint_move[3] = joint_allstop[3];
      joint_move[4] = joint_allstop[4];
      break;
    }

    default:
      break;
    }
    // // 一号关节
    // joint_move[0] = joint_move1[0];
    // // 二号关节
    // joint_move[1] = joint_move2[1];
    // // 三号关节
    // joint_move[2] = joint_move3[2];
    // // 四号关节
    // joint_move[3] = joint_move4[3];
    // // 泵
    // joint_move[4] = close_pump[0];
    int w;
    printf("\n 综合控制帧为：");
    for (w = 0; w < 5; w++)
    {
      printf("0x%02X ", joint_move[w]);
    }
    printf("\n");
    SendData(can0_socket, ext_id, true, joint_move, 5);
  }
};

int main(int argc, char const *argv[])
{
  // 2.初始化ROS2客户端；
  rclcpp::init(argc, argv);
  // rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20, true);
  rclcpp::executors::MultiThreadedExecutor executor;
  // 初始化CAN0接口
  can0_socket = init_can("can0");
  if (can0_socket < 0)
  {
    std::cerr << "CAN0初始化失败，程序退出！" << std::endl;
    return -1;
  }
  auto node = std::make_shared<Arm_Control>();
  // 4.调用spain函数，并传入节点对象指针；
  executor.add_node(node);
  executor.spin();
  // 5.资源释放
  rclcpp::shutdown();
  // 全部停止
  SendData(can0_socket, ext_id, true, joint_stop, 5);
  // 关闭CAN设备
  close(can0_socket); // 关闭CAN接口
  return 0;
}
